#include"multicam_fusion.h"

#define GRAVITATIONAL_ACC 9.794 // The gravity of Shanghai

/**
 * @brief 构造函数载入参数,初始化矩阵 Construct a new dwe::dwe object
 */
dwe::dwe(ros::NodeHandle& nh) {

    ROS_INFO("**********START ROS NODE**********");
    // br_uav_to_origin = br;

    //============================== Read ros parameter ===========================//
    const std::string& node_name = ros::this_node::getName();
    nh.getParam(node_name + "/" + "dataType", dataType_);

    // param for MAP sliding window
    nh.getParam(node_name + "/" + "mapwin/delta", delta_);
    nh.getParam(node_name + "/" + "mapwin/lopt",  lopt_);
    nh.getParam(node_name + "/" + "mapwin/order", order_);
    nh.getParam(node_name + "/" + "mapwin/k", k_);
    nh.getParam(node_name + "/" + "mapwin/rate", rate_);
    nh.getParam(node_name + "/" + "mapwin/init_num", init_num_);
    nh.getParam(node_name + "/" + "mapwin/vicon_flag", vicon_flag_);
    nh.getParam(node_name + "/" + "mapwin/param_flag", param_flag_);

    // param for dwe_param
    nh.getParam(node_name + "/" + "dwe_param/paramPp",  paramPp_);
    nh.getParam(node_name + "/" + "dwe_param/paramPv",  paramPv_);
    nh.getParam(node_name + "/" + "dwe_param/paramQp",  paramQp_);
    nh.getParam(node_name + "/" + "dwe_param/paramQv",  paramQv_);
    nh.getParam(node_name + "/" + "dwe_param/paramR",  paramR_);
    nh.getParam(node_name + "/" + "dwe_param/paramU",  paramU_);
    nh.getParam(node_name + "/" + "dwe_param/paramUz", paramUz_);
    nh.getParam(node_name + "/" + "dwe_param/paramC",  paramC_);
    nh.getParam(node_name + "/" + "dwe_param/paramCz", paramCz_);

    vector<double> kappa_vec(3);
    nh.getParam(node_name + "/" + "dwe_param/kappa", kappa_vec);
    for (int i = 0; i < 3; i++) {
        kappa_(i) = kappa_vec[i];
    }

    // param for msg_sensor
    nh.getParam(node_name + "/" + "msg_sensor/camnum", camnum_);
    nh.getParam(node_name + "/" + "msg_sensor/usedcamnum", usedcamnum_);

    // param for msg_bias
    bias_cam_ = Eigen::MatrixXd(3, camnum_);

    vector<double> bias_cam_vec(3*camnum_);
    nh.getParam(node_name + "/" + "msg_bias/bias_cam", bias_cam_vec);
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < camnum_; j++) {
            bias_cam_(i,j) = bias_cam_vec[camnum_ * i + j];
        }
    }

    vector<double> bias_marker_vec(3);
    nh.getParam(node_name + "/" + "msg_bias/bias_marker", bias_marker_vec);
    for (int i = 0; i < 3; i++) {
        bias_marker_(i) = bias_marker_vec[i];
    }

    // pos_uav_init_vec 是[无人机mark标记]  在 [VICON坐标系] 下的位置
    // pos_uav_init_    是[无人机中心] 在 [VICON坐标系] 下的位置
    vector<double> pos_uav_init_vec(3);
    nh.getParam(node_name + "/" + "msg_sensor/pos_uav_init", pos_uav_init_vec);
    for (int i = 0; i < 3; i++) {
        pos_uav_init_(i) = pos_uav_init_vec[i] + bias_marker_(i);
    }

    // param for msg_bias
    nh.getParam(node_name + "/" + "msg_bias/bias_imu", bias_imu_);

    vector<double> bias_noop_vec(3);
    nh.getParam(node_name + "/" + "msg_bias/bias_noop", bias_noop_vec);
    for (int i = 0; i < 3; i++) {
        bias_noop_(i) = bias_noop_vec[i];
    }

    vector<double> bias_pos_vec(3);
    nh.getParam(node_name + "/" + "msg_bias/bias_pos", bias_pos_vec);
    for (int i = 0; i < 3; i++) {
        bias_pos_(i) = bias_pos_vec[i];
    }
    //========================== Initialize matrix param ===========================//
    // window size
    LOOP_num_ = 1;
    num_state_= k_*lopt_; 
    count_j_ = num_state_ + delta_;   // 8+3
    startNum_ = init_num_*num_state_; // start before estimation

    // 根据传感器数量初始化回调函数参数
    cam_cbs_.resize(camnum_);
    cam_data_received_.assign(camnum_, false);
    sensor = SensorParams(camnum_);
    // 初始化对应矩阵权重
    paramX0_  = Eigen::MatrixXd::Zero(6, num_state_);  // 初始化为 0
    paramIMU_ = Eigen::MatrixXd::Zero(6, num_state_);  // 初始化为 0
    paramX0_.block(0, 0, 3, num_state_) = Eigen::MatrixXd::Constant(3, num_state_, paramPp_);
    paramX0_.block(3, 0, 3, num_state_) = Eigen::MatrixXd::Constant(3, num_state_, paramPv_);
    paramIMU_.block(0, 0, 3, num_state_)= Eigen::MatrixXd::Constant(3, num_state_, paramQp_);
    paramIMU_.block(3, 0, 3, num_state_)= Eigen::MatrixXd::Constant(3, num_state_, paramQv_);

    paramCAM_  = Eigen::MatrixXd::Zero(3*camnum_, num_state_+1);  // 初始化为 0
    for (int i=0;i<camnum_;i++){
        paramCAM_.block(3*i, 0, 2, num_state_+1) = Eigen::MatrixXd::Constant(2, num_state_+1, paramC_);
        paramCAM_.block(3*i+2, 0, 1, num_state_+1) = Eigen::MatrixXd::Constant(1, num_state_+1, paramCz_);
    }
    sensor.param.cam = paramCAM_.rightCols(1);
    sensor.param.sum = sensor.param.cam.sum();
    // 初始化测量值矩阵
    MHE_imu_.setOnes(3,num_state_+1);
    MHE_imu_att_.setOnes(4,num_state_+1);
    MHE_imu_residual_.setZero(3,num_state_+1);

    MHE_cam_.setZero(3*camnum_,num_state_+1);
    MHE_cam_residual_.setZero(3*camnum_,num_state_+1);
    deltaTime_.setZero(1,num_state_+1);
    // MHE_flow_.setOnes(3,k_*num_state_+1);
    // MHE_height_.setZero(1,num_state_+1);

    // 初始化估计值矩阵
    xt_.setOnes(6,num_state_+1);
    xt_vel_.setZero(3,num_state_+1);
    xt_real_.setOnes(6,num_state_+1);
    x1_.setOnes(6,num_state_+1);
    xt_real_f_.setOnes(6,num_state_+1);
    xt_f_.setOnes(6,num_state_+1);

    // 初始化真值参考矩阵
    VICON_PosUAV_.setZero(3,num_state_+1);
    VICON_VelUAV_.setOnes(3,num_state_+1);

    current_cam_stamp.setZero(1,camnum_); 
    last_cam_stamp.setZero(1,camnum_); 
    last_update_time.setZero(1,camnum_); 

    // xpre(6*6),u(6*6),r(1*1*num),cam_pos(3*3*num),uwb_pos(3*3)
    Ex.setZero((6+6+usedcamnum_*3)*num_state_, 6*(num_state_+1));
    // xpre(6*6),u(6*6),r(1*1*num),cam_pos(3*3*num),uwb_pos(3*3)
    Eu.setZero((6+6+usedcamnum_*3)*num_state_, (6+3+usedcamnum_*3)*num_state_);
    // xpre(6*6),u(6*6),r(1*1*num),cam_pos(3*3*num),uwb_pos(3*3)
    W.setZero((6+6+usedcamnum_*3)*num_state_, (6+6+usedcamnum_*3)*num_state_);
    // xpre(6*1),u(3*1),r(1*num),cam_pos(3*num),uwb_pos(3*1)
    ur.setZero((6+3+3*usedcamnum_)*num_state_,1);
    xxe.setZero(6,num_state_);                              //6x20

    t_matrix.setZero(6*(num_state_+1),6*(order_+1));   //126x30
    for (int j=1;j<=6;j++){
        t_matrix(j-1,(j-1)*(order_+1)) = 1;
    }

}

/**
 * @brief 析构函数
 */
dwe::~dwe(){
}

/**
 * @brief main cb
 */
void dwe::imu_new_cb(const sensor_msgs::Imu::ConstPtr &msg){
    // 至少在有imu数据时才会开始估计
    // ROS_WARN("---------- IMU ----------");
   
    imu_msg = *msg;

   // for (int i = 1; i <= 3; i++) {
        // tiao bian
    //    double diff_imu = abs(MHE_imu_(i-1, num_state_) - MHE_imu_(i-1, num_state_ - 1));
      //  double diff0_imu = abs(MHE_imu_(i-1, num_state_) - MHE_imu_(i-1, 0));

       // if (hasStartEst_) { // If the drone has taken off and diff0 is not zero
         //   if (diff_imu >= 1.2 && diff0_imu != 0) { // If the difference is big
           //     MHE_imu_(i-1, num_state_) = MHE_imu_(i-1, num_state_ - 1); // Set current value to the previous value
           // }
            // else if (diff_imu >= 1.0) { // If the difference is small or long time lost
            //     // MHE_imu_.row(i-1) = meanFilter(MHE_imu_.row(i-1)); // Apply mean filter
            //     MHE_imu_(i-1, num_state_) = (MHE_imu_(i-1, num_state_) + MHE_imu_(i-1, num_state_-1)) /2; // Apply mean filter
            //     // Eigen::MatrixXd MHE_imu_meanFilter = meanFilter(MHE_imu_.row(i-1));
            //     // MHE_imu_(i-1, num_state_)= MHE_imu_meanFilter(0,num_state_);
            // }
       // }
   // }
}

/**
 * @brief initial data xt_ with vicon
 */
void dwe::Initialdataxt(){
    ROS_INFO("---------- Initialize with vicon ----------");
    // initialize loc with vicon, use UWB01 as initial point
    xt_.block<3,1>(0, num_state_) = VICON_PosUAV_.col(num_state_);
    xt_.block<3,1>(3, num_state_) = VICON_VelUAV_.col(num_state_);
    // initialize loc with vicon
    xt_real_.block<3,1>(0, num_state_) = VICON_PosUAV_.col(num_state_);
    xt_real_.block<3,1>(3, num_state_) = VICON_VelUAV_.col(num_state_);

    // use real_est to compare [vicon坐标系]
    xt_real_f_.rightCols(1) = xt_real_.rightCols(1);
    xt_f_.rightCols(1) = xt_.rightCols(1);
    // // use relative_est to compare [UWB0坐标系]
    // xt_real_f_.rightCols(1) = xt_.rightCols(1);
    // Initialize velocity with VICON
    xt_vel_.block<3,1>(0, num_state_) = VICON_VelUAV_.col(num_state_);
} 
/**
 * @brief initial data xt_ with pre-get param
 */

// todo
void dwe::InitialWithParam(){
    ROS_INFO("---------- Initialize with param----------");
    // xt_ 是[无人机中心(IMU)] 在 [UWB0坐标系] 下的位置
    xt_.block<3,1>(0, num_state_) = VICON_PosUAV_.col(num_state_);
    xt_.block<3,1>(3, num_state_).setZero();

    // xt_real_ 是[无人机中心(IMU)] 在 [VICON坐标系] 下的位置
    xt_real_.block<3,1>(0, num_state_) = pos_uav_init_;
    xt_real_.block<3,1>(3, num_state_).setZero();
    
    // use real_est to compare [vicon坐标系]
    xt_real_f_.rightCols(1) = xt_real_.rightCols(1);
    xt_f_.rightCols(1) = xt_.rightCols(1);
    // // use relative_est to compare [UWB0坐标系]
    // xt_real_f_.rightCols(1) = xt_.rightCols(1);

    // Initialize xt_vel_ with zeros
    xt_vel_.col(num_state_).setZero();
} 


/**
 * @brief 初始化估计
 */
void dwe::initializeEstimation() {
    // if (LOOP_num_ > count_j_ + 1 && LOOP_num_ == startNum_) {
    //     hasStartEst_ = true;
    // }
    if (VICON_PosUAV_(2,num_state_) >=0.34) {
        hasStartEst_ = true;
    }
    if (!hasStartEst_) {
        param_flag_ ? Initialdataxt() : InitialWithParam();
        
        // 展示初始化:[无人机中心(IMU)] 在 [VICON坐标系] 下的位置
        ROS_INFO("Initialized data: x:%f y:%f z:%f", xt_(0, num_state_), xt_(1, num_state_), xt_(2, num_state_));
        ROS_INFO("dt: %.4lf seconds", dt_);
    }
}

/**
 * @brief 数据缺失检测
 */
int data_loss_detection(const Eigen::MatrixXd& MHE_data_, double loss_epsilon) {
    double sum_of_squares = 0;
    int MHE_data_num = MHE_data_.cols(); // 获取列数作为 num_state_
    
    // 计算各元素的平方和
    for (int i = 0; i < MHE_data_num; i++) {
        sum_of_squares += MHE_data_(0, i) * MHE_data_(0, i);
    }
    
    // 判断是否符合条件
    if (double (sum_of_squares / MHE_data_num) < loss_epsilon) {
        return 0; // lost
    } else {
        return 1;
    }
}


/**
 * @brief 更新参数与移位操作
 */
void dwe::update_data() {
    //======================================移位==================================//
    xt_.leftCols(num_state_)=xt_.rightCols(num_state_);
    xt_real_.leftCols(num_state_)=xt_real_.rightCols(num_state_);
    xt_real_f_.leftCols(num_state_)=xt_real_f_.rightCols(num_state_);
    xt_f_.leftCols(num_state_)=xt_f_.rightCols(num_state_);
    xt_vel_.leftCols(num_state_)=xt_vel_.rightCols(num_state_);

    // flag_init_imu_ = true;
    // Calculate the dt_
    currentTime_= ros::Time::now();

    if (previousTime_ != ros::Time(0)) {
        deltaTime_.leftCols(num_state_) = deltaTime_.rightCols(num_state_);
        deltaTime_(0, num_state_) = (currentTime_ - previousTime_).toSec();
    }
    previousTime_ = currentTime_;

    if (LOOP_num_ >= num_state_ + 1) {
        dt_ = deltaTime_.sum() / (num_state_ + 1);
    } else {
        dt_ = deltaTime_.sum() / LOOP_num_;
    }
    // get data for every spin

    // 初始化估计
    if (!hasStartEst_) {
        initializeEstimation();
    } else {
        flag_init_ok_ = true;
        ROS_WARN("----------- Start Estimate -----------");
    }

    A << 1, 0, 0, dt_, 0, 0,
                0, 1, 0, 0, dt_, 0,
                0, 0, 1, 0, 0, dt_,
                0, 0, 0, 1-kappa_(0)*dt_, 0, 0,
                0, 0, 0, 0, 1-kappa_(1)*dt_, 0,
                0, 0, 0, 0, 0, 1-kappa_(2)*dt_;

    B << 0.5*pow(dt_,2), 0, 0,
                    0, 0.5*pow(dt_,2), 0,
                    0, 0, 0.5*pow(dt_,2),
                    dt_, 0,  0,  
                    0,  dt_, 0,
                    0,  0, dt_; 
    //==================================IMU 数据处理==================================//
    MHE_imu_.leftCols(num_state_) = MHE_imu_.rightCols(num_state_);
    MHE_imu_att_.leftCols(num_state_) = MHE_imu_att_.rightCols(num_state_);
    rotation_matrix = quaternion2mat(MHE_imu_att_.col(num_state_));

    MHE_imu_(0,num_state_) = imu_msg.linear_acceleration.x * GRAVITATIONAL_ACC;
    MHE_imu_(1,num_state_) = imu_msg.linear_acceleration.y * GRAVITATIONAL_ACC;
    MHE_imu_(2,num_state_) = imu_msg.linear_acceleration.z * GRAVITATIONAL_ACC;

    MHE_imu_att_(0, num_state_) = imu_msg.orientation.w;
    MHE_imu_att_(1, num_state_) = imu_msg.orientation.x;
    MHE_imu_att_(2, num_state_) = imu_msg.orientation.y;
    MHE_imu_att_(3, num_state_) = imu_msg.orientation.z;

    MHE_imu_.col(num_state_) = rotation_matrix* MHE_imu_.col(num_state_);
    MHE_imu_(2, num_state_) += bias_imu_;

    cov_pre_xt_new = A * xt_.block(0, 0, 6, num_state_+1) + 
                    B * MHE_imu_.block(0, 0, 3, num_state_+1);

    //*****************imu: lost detection
    MHE_imu_residual_.col(num_state_) = (MHE_imu_.col(num_state_) - 
                                        MHE_imu_.col(num_state_-1)).cwiseAbs();
    sensor.fail.imu = data_loss_detection(MHE_imu_residual_.row(0),residual_threshold);

    // //*****************imu: quality detection
    // double k_imu = 10.0; // 控制 Sigmoid 曲线的陡度
    // double x0_imu = 0.30; // 控制 Sigmoid 曲线的中心点
    // sensor.quality.imu = Eigen::Vector3d::Ones() - 
    //                     sigmoid_eigen(MHE_imu_residual_.col(num_state_), k_imu, x0_imu);

    //==================================CAM ABCD 数据处理==================================//
    MHE_cam_.leftCols(num_state_)=MHE_cam_.rightCols(num_state_);
    MHE_cam_residual_.leftCols(num_state_)=MHE_cam_residual_.rightCols(num_state_);
    int cam_fail_flag = 0;
    sensor.weight.cam = sensor.param.cam;

    for (int i=0;i<camnum_;i++){
        // Update parameters with new data
        if (std::isnan(cam_cbs_[i].transform.translation.x)) {
            cam_cbs_[i].transform.translation.x = 0;
            cam_cbs_[i].transform.translation.y = 0;
            cam_cbs_[i].transform.translation.z = 0;
        }
        MHE_cam_(3 * i, num_state_)     = cam_cbs_[i].transform.translation.x + bias_cam_(0,i);
        MHE_cam_(3 * i + 1, num_state_) = cam_cbs_[i].transform.translation.y + bias_cam_(1,i);
        MHE_cam_(3 * i + 2, num_state_) = cam_cbs_[i].transform.translation.z + bias_cam_(2,i); 

        //--------calibrate now
        MHE_cam_.block<3, 1>(3 * i, num_state_) =  MHE_cam_.block<3, 1>(3 * i, num_state_);

        //*****************cam: lost detection
        MHE_cam_residual_.block(3 * i,num_state_,3,1) = (MHE_cam_.block(3 * i,num_state_,3,1) - 
                                MHE_cam_.block(3 * i,num_state_-1,3,1)).cwiseAbs();
        sensor.fail.cam[i] = data_loss_detection(MHE_cam_residual_.row(i),residual_threshold);

        // 差值计算
        Eigen::Vector3d diff = MHE_cam_residual_.block<3, 1>(3 * i, num_state_);
        Eigen::Vector3d diffxt = (MHE_cam_.block<3, 1>(3 * i, num_state_) - xt_.block<3, 1>(0, num_state_)).cwiseAbs();
        double diffx0 = abs(MHE_cam_(3 * i+2, num_state_-1) - MHE_cam_(3 * i+2, 0));

        
        // ROS_INFO("DATA diffxtx:%f diffxty:%f diffxtz:%f", diffxt[0],diffxt[1],diffxt[2]);
        // ROS_WARN("DATA MHE_cam%d: x:%f y:%f z:%f",i,MHE_cam_(3 * i, num_state_),MHE_cam_(3 * i + 1, num_state_),MHE_cam_(3 * i + 2, num_state_));
        // ROS_WARN("DATA xt: x:%f y:%f z:%f",xt_(0, num_state_),xt_(1, num_state_),xt_(2, num_state_));

        if (hasStartEst_) {  // 如果无人机已经起飞并且diff0不为零
            bool diff_zero = (diff.array() == 0.000000).any();
            bool diff_large = (diff.array() >= 0.20).any();
            bool diffxt_large = (diffxt.array() >= 0.25).any();
        
            // 如果长时间没有变化,或者突变很大,则说明相机数据无效,保留当前值
            // if (diff.isZero() && (diffx0 == 0.000000) && (diffxt[2]>=0.10 || diffxt[0]>=0.20 || diffxt[1]>=0.20)) {
            if (diff_zero) {
                sensor.weight.cam.block<3, 1>(3 * i, 0) << paramC_ * 0.2, paramC_ * 0.2, paramCz_*0.2;

                if(diffx0 == 0.000000 || diff_large || diffxt_large){
                    // MHE_cam_.block<3, 1>(3 * i, num_state_) = MHE_cam_.block<3, 1>(3 * i, num_state_ - 1);
                    ROS_WARN("CAM %d failed, diffxt[0] = %.4f, diffxt[1] = %.4f, diffxt[2] = %.4f",i+1, diffxt[0],diffxt[1],diffxt[2] );
                    sensor.weight.cam.block<3, 1>(3 * i, 0) << 0.00, 0.00, 0.00;
                    // ROS_INFO("DATA xt_: x:%f y:%f z:%f",sensor.weight.cam(3 * i,0),sensor.weight.cam(3 * i+1,0),sensor.weight.cam(3 * i+2,0));
                    cam_fail_flag ++;
                    continue;
                }
            }

            // 可能长时间丢失,和估计值差别不大,保留该值
            if(!diff_zero && (diffxt.array() <= 0.10).any() && diffx0 == 0.00000){
                sensor.weight.cam.block<3, 1>(3 * i, 0) << paramC_ * 0.1, paramC_ * 0.1, paramCz_*0.1;
                continue;
            }

            // 如果差值较小时间丢失
            if ((diff.array() >= 0.070).any() && (diff.array() < 0.12).any()) {
            // if ((diff.array() >= 0.070).any() && (diffx0 != 0.000000)) {
                // MHE_cam_.block<3, 1>(3 * i, num_state_) = (MHE_cam_.block<3, 1>(3 * i, num_state_) + MHE_cam_.block<3, 1>(3 * i, num_state_ - 1)) / 2;
                sensor.weight.cam.block<3, 1>(3 * i, 0) << paramC_ * 0.5, paramC_ * 0.5, paramCz_*0.5;
                // continue;
            }
            // 如果差值很大
            else if ((diff.array() >= 0.12).any() && (diff.array() < 0.20).any()) {
            // if ((diff.array() >= 0.15).any() && (diffx0 != 0.000000)) {
                // MHE_cam_.block<3, 1>(3 * i, num_state_) = MHE_cam_.block<3, 1>(3 * i, num_state_ - 1);
                sensor.weight.cam.block<3, 1>(3 * i, 0) << paramC_ * 0.1, paramC_ * 0.1, paramCz_*0.1;
                // continue;
            }
            else if (diff_large) {
            // if ((diff.array() >= 0.15).any() && (diffx0 != 0.000000)) {
                MHE_cam_.block<3, 1>(3 * i, num_state_) = MHE_cam_.block<3, 1>(3 * i, num_state_ - 1);
                sensor.weight.cam.block<3, 1>(3 * i, 0) << paramC_ * 0.00, paramC_ * 0.00, paramCz_*0.00;
                continue;
            }

            for (int axis = 0; axis < 3; ++axis) {
                double paramC_value = (axis == 2) ? paramCz_ : paramC_;  // 根据轴选择 paramC 或 paramCz
                double scale_factor = 1.0;

                if (diffxt[axis] >= 0.10 && diffxt[axis] < 0.15) {
                    scale_factor = 0.4;
                } else if (diffxt[axis] >= 0.15 && diffxt[axis] < 0.20) {
                    scale_factor = 0.2;
                } else if (diffxt[axis] >= 0.20 && diffxt[axis] < 0.25) {
                    scale_factor = 0.01;
                    MHE_cam_.block<3, 1>(3 * i, num_state_) = MHE_cam_.block<3, 1>(3 * i, num_state_ - 1);
                } else if (diffxt[axis] >= 0.25) {
                    MHE_cam_.block<3, 1>(3 * i, num_state_) = MHE_cam_.block<3, 1>(3 * i, num_state_ - 1);
                    scale_factor = 0.00;
                }
                // ROS_WARN("DATA MHE_cam%d: x:%f y:%f z:%f",i,MHE_cam_(3 * i, num_state_),MHE_cam_(3 * i + 1, num_state_),MHE_cam_(3 * i + 2, num_state_));
                // ROS_WARN("DATA xt_: x:%f y:%f z:%f",xt_(0, num_state_),xt_(1, num_state_),xt_(2, num_state_));
                // ROS_WARN("diffxt[%d]: %f",axis,diffxt[axis]);
                // ROS_WARN("scale_factor: %f",scale_factor);
                sensor.weight.cam(3 * i+axis, 0)= paramC_value * scale_factor;
            }
        }

        // MHE_cam_.rightCols(1) = meanFilter(MHE_cam_.rightCols(3)).rightCols(1);
        // ROS_INFO("DATA MHE_cam%d: x:%f y:%f z:%f",i,MHE_cam_(3 * i, num_state_),MHE_cam_(3 * i + 1, num_state_),MHE_cam_(3 * i + 2, num_state_));
        // ROS_INFO("DATA xt_: x:%f y:%f z:%f",i,xt_(0, num_state_),xt_(1, num_state_),xt_(2, num_state_));

    }
    // // 0410
    // if(xt_(2,num_state_)>=2.0){
    //     MHE_cam_.rightCols(1) = meanFilter(MHE_cam_.rightCols(3)).rightCols(1);
    // }
    
    // paramCAM_(0,num_state_) = 1.2 * paramCAM_(0,num_state_);
    // paramCAM_(1,num_state_) = 1.2 * paramCAM_(1,num_state_);
    // paramCAM_(2,num_state_) = 1.2 * paramCAM_(2,num_state_);

    // if camz all failed 
    if(cam_fail_flag==usedcamnum_){
        // paramX0_(2, num_state_-1)  = paramP_ * 2;
        // paramX0_(5, num_state_-1)  = paramP_ * 2;
        // paramIMU_(2, num_state_-1) = paramQ_ * 2;
        // paramIMU_(5, num_state_-1) = paramQ_ * 2;
        ROS_WARN("ALL CAM failed!!!!!");
        // paramUWB_.rightCols(1) = 0.01 * paramUWB_.rightCols(1);
    }
    // sensor.weight.cam.block<3, 1>(12, 0) << 0.00, 0.00, 0.00;

    // // Add the factor 
    // ROS_WARN("cam_fail_flag: %d",cam_fail_flag);
    // if (cam_fail_flag != 0) {
    //     for (int i = 0; i < usedcamnum_; i++) {
    //         // 提取 paramCAM_ 的块 (3*i 行, num_state_ 列)，大小为 3x1
    //         Eigen::Matrix<double, 3, 1> column = paramCAM_.block<3, 1>(3 * i, num_state_);
            
    //         // 检查该块是否为非零列
    //         if (column != Eigen::Matrix<double, 3, 1>::Zero()) {
    //             // 修改该块的值
    //             paramCAM_.block<3, 1>(3 * i, num_state_) =
    //                 column * usedcamnum_ / (usedcamnum_ - cam_fail_flag);
    //             // ROS_WARN("usedcamnum_: %d",usedcamnum_);
    //             // ROS_WARN("usedcamnum_ * (usedcamnum_ - cam_fail_flag): %d",usedcamnum_ * (usedcamnum_ - cam_fail_flag));
    //         }
    //     }
    // }

    // smooth
    // paramCAM_.rightCols(1) = meanFilter(paramCAM_.rightCols(3)).rightCols(1);

    cam_fail_flag = 0;

    //==================================set param==================================//
    paramCAM_.leftCols(num_state_)=paramCAM_.rightCols(num_state_);
    // paramIMU_.leftCols(num_state_-1)=paramIMU_.rightCols(num_state_-1);

//    // ============== adaptive param for all================
//     sensor.weight.sum = sensor.weight.cam.sum() + sensor.weight.uwb.sum();
//     if (sensor.weight.sum == 0) {
//         sensor.weight.sum = 1e-6;  // 避免除零错误
//     }

   
//     // 辅助函数：归一化并限制范围
//     auto normalizeAndLimit = [&](double weight) {
//         return std::min((weight / sensor.weight.sum) * sensor.param.sum, 100.0);
//     };

//     for (int i = 0; i < usedcamnum_; ++i) {
//         for (int j = 0; j < 3; ++j) {
//             paramCAM_(3*i+j, num_state_) = normalizeAndLimit(sensor.weight.cam(3*i+j,0));
//         }
//     }

//     for (int i = 0; i < useduwbnum_; ++i) {
//         paramUWB_(i, num_state_) = normalizeAndLimit(sensor.weight.uwb[i]);
//     }

   // ============== adaptive param
    sensor.weight.sum = sensor.weight.cam.sum();
    if (sensor.weight.sum == 0) {
        sensor.weight.sum = 1e-3;  // 避免除零错误
    }
    double cam_sum = sensor.weight.cam.sum();
    if(cam_sum == 0) {
        cam_sum = 1e-3;  // 避免除零错误
    }

    // 辅助函数：归一化并限制范围
    auto normalizeAndLimit = [&](double weight,double weight_sum,double param_sum) {
        return std::min((weight / weight_sum) * param_sum, 100.0);
    };

    // int rows = sensor.weight.cam.rows();
    // int cols = sensor.weight.cam.cols();
    // ROS_WARN("===============rows:%d, cols:%d================",rows,cols);
    // int prows = sensor.param.cam.rows();
    // int pcols = sensor.param.cam.cols();
    // ROS_WARN("===============param rows:%d, cols:%d================",prows,pcols);

    for (int i = 0; i < usedcamnum_; ++i) {
        for (int j = 0; j < 3; ++j) {
            paramCAM_(3*i+j, num_state_) = normalizeAndLimit(sensor.weight.cam(3*i+j,0),cam_sum,sensor.param.cam.sum());
        }
    }

    // Eigen::MatrixXd paramCAM_meanFilter_1 = meanFilter(paramCAM_.rightCols(3));
    // paramCAM_.rightCols(1) = paramCAM_meanFilter_1.rightCols(1);

    // Eigen::MatrixXd paramCAM_meanFilter_2 = meanFilter(paramCAM_.rightCols(3));
    // paramCAM_.rightCols(1) = paramCAM_meanFilter_2.rightCols(1);
    //==================================vicon posuav 数据处理==================================//
    // shift
    VICON_PosUAV_.leftCols(num_state_)=VICON_PosUAV_.rightCols(num_state_);
    // !!!!!! note: mark bias for nokov
    Eigen::MatrixXd bias_rotation = rotation_matrix * bias_marker_.transpose();
    VICON_PosUAV_(0,num_state_)=VICON_posUAV_msg.pose.position.x + bias_rotation(0);
    VICON_PosUAV_(1,num_state_)=VICON_posUAV_msg.pose.position.y + bias_rotation(1);
    VICON_PosUAV_(2,num_state_)=VICON_posUAV_msg.pose.position.z + bias_rotation(2);

    // ===================================vicon veluav
    VICON_VelUAV_.leftCols(num_state_)=VICON_VelUAV_.rightCols(num_state_);
    VICON_VelUAV_(0,num_state_)=VICON_velUAV_msg.twist.linear.x;
    VICON_VelUAV_(1,num_state_)=VICON_velUAV_msg.twist.linear.y;
    VICON_VelUAV_(2,num_state_)=VICON_velUAV_msg.twist.linear.z;

    ROS_INFO("EST MHE_cam1_: cam01_x:%f cam01_y:%f cam01_z:%f",
        MHE_cam_(0,num_state_),
        MHE_cam_(1,num_state_),
        MHE_cam_(2,num_state_));
    ROS_INFO("EST MHE_cam2_: cam02_x:%f cam02_y:%f cam02_z:%f",
        MHE_cam_(3,num_state_),
        MHE_cam_(4,num_state_),
        MHE_cam_(5,num_state_));
    ROS_INFO("EST MHE_cam3_: cam03_x:%f cam03_y:%f cam03_z:%f",
        MHE_cam_(6,num_state_),
        MHE_cam_(7,num_state_),
        MHE_cam_(8,num_state_));
    ROS_INFO("EST MHE_cam4_: cam04_x:%f cam04_y:%f cam04_z:%f",
        MHE_cam_(9,num_state_),
        MHE_cam_(10,num_state_),
        MHE_cam_(11,num_state_));
    ROS_INFO("EST MHE_camP_: camP_x:%f camP_y:%f camP_z:%f",
        MHE_cam_(12,num_state_),
        MHE_cam_(13,num_state_),
        MHE_cam_(14,num_state_));
    ROS_INFO("EST MHE_camT_: camT_x:%f camT_y:%f camT_z:%f",
        MHE_cam_(15,num_state_),
        MHE_cam_(16,num_state_),
        MHE_cam_(17,num_state_));
    ROS_INFO("Param MHE_cam2_: cam02_x:%f cam02_y:%f cam02_z:%f",
        paramCAM_(3,num_state_),
        paramCAM_(4,num_state_),
        paramCAM_(5,num_state_));

}


/**
 * @brief 进行滑窗内的估计
 */
void dwe::solveonce()
{
    ROS_INFO("dt: %.4lf seconds", dt_); 

    // Initialize matrix in solveonce
    x1_= xt_.rightCols(k_*num_state_+1);
    // MatrixXd x0 = xt_.rightCols(k_*num_state_+2);
    // x1_= x0.leftCols(k_*num_state_+1);
    
    // use vicon
    MatrixXd imu_sol;
    MatrixXd cam_sol;

    int camnum_solveonce = usedcamnum_;
    if (vicon_flag_ == 1)
    {
        // imu_sol = VICON_ImuUAV_.leftCols(k_*num_state_); //imu from vicon
        imu_sol = MHE_imu_.leftCols(k_*num_state_); //imu from vicon
        
        // cam_sol = MHE_cam_.topRows(3*usedcamnum_);
        cam_sol = Eigen::MatrixXd::Zero(3*camnum_solveonce,num_state_+1);
        // cam_sol.block(0,0,3,num_state_+1) =  VICON_PosUAV_ - VICON_PosUWB_.topRows(3);
        // cam_sol.block(3,0,3,num_state_+1) =  VICON_PosUAV_ - VICON_PosUWB_.topRows(3);
    }
    else{
        // use sensor
        imu_sol = MHE_imu_.leftCols(k_*num_state_); //imu from uav
        // imu_sol = VICON_ImuUAV_.leftCols(k_*num_state_); //imu from vicon
        cam_sol = MHE_cam_.topRows(3*camnum_solveonce);

        // uwb A front
        // paramUWB_.row(6) = Eigen::MatrixXd::Constant(1, num_state_+1, 0);
        // paramUWB_.row(4) = Eigen::MatrixXd::Constant(1, num_state_+1, 0);
        // paramUWB_.row(0) = Eigen::MatrixXd::Constant(1, num_state_+1, 0);
        // paramUWB_.row(3) = Eigen::MatrixXd::Constant(1, num_state_+1, 0);
        // paramUWB_.row(7) = Eigen::MatrixXd::Constant(1, num_state_+1, 0);

        // paramUWB_.row(7) = Eigen::MatrixXd::Constant(1, num_state_+1, 0);
        // uwb B back
        // paramUWB_.row(1) = Eigen::MatrixXd::Constant(1, num_state_+1, 0);
        // paramUWB_.row(6) = Eigen::MatrixXd::Constant(1, num_state_+1, 0);
        // cam_sol.row(2) =  VICON_PosUAV_.row(2)- VICON_PosUWB_.row(2);

        // cam_sol.row(5) =  VICON_PosUAV_.row(2)- VICON_PosUWB_.row(2);
        // paramCAM_.row(12).setConstant(0.0);
        // paramCAM_.row(13).setConstant(0.0);
        // paramCAM_.row(14).setConstant(0.0);
        // cam_sol.row(8) =  VICON_PosUAV_.row(2)- VICON_PosUWB_.row(2);
        // paramCAM_.row(8).setConstant(10.0);
        // cam_sol.row(11) = VICON_PosUAV_.row(2)- VICON_PosUWB_.row(2);
        // paramCAM_.row(11).setConstant(10.0);
    }
    
    AA = A;
    for (int i = 1; i < k_; i++) {
        AA = AA*A;
    }  
    // double test_time_start_1 = ros::Time::now().toSec();   //test

    // // we can design a member function to perform a shift operation
    Eigen::MatrixXd Cc[camnum_solveonce];
    // ROS_WARN("%f,%f,%f,%f,%f,%f",paramIMU_(0,num_state_-1),paramIMU_(1,num_state_-1),paramIMU_(2,num_state_-1),paramIMU_(3,num_state_-1),paramIMU_(4,num_state_-1),paramIMU_(5,num_state_-1));
    for (int i=1;i<=num_state_;i++){ //i=1-20 or 0-19
        // x0- ---- Ex Eu W
        Ex.block((i-1)*6,(i-1)*6,6,6)=Eigen::MatrixXd::Identity(6,6);
        Eu.block((i-1)*6,(i-1)*6,6,6) = Eigen::MatrixXd::Identity(6,6);
        W.block((i-1)*6,(i-1)*6,6,6) = paramX0_.col(i-1).asDiagonal();

        // imu ---- Ex Eu W
        Ex.block((i-1)*6+6*num_state_,(i-1)*6,6,6) = -AA;
        Ex.block((i-1)*6+6*num_state_,(i)*6,6,6) = Eigen::MatrixXd::Identity(6,6);
        for(int j=1;j<=k_;j++){
            Eigen::Matrix<double, 6, 6> Akj;
            Akj.setIdentity(6,6);
            int kj=k_-j;
            for (int q=0;q<kj;q++){
                Akj=Akj*A;
            }
            Eu.block(6*num_state_+(i-1)*6,6*num_state_+(i-1)*3*k_+(j-1)*3,6,3) = Akj*B;
        }
        W.block(6*num_state_+(i-1)*6,6*num_state_+(i-1)*6,6,6) = paramIMU_.col(i-1).asDiagonal();
        
        for (int j = 1; j <=camnum_solveonce; j++) {
            // cam ---- Ex
            Cc[j-1] = Eigen::MatrixXd::Zero(3, 6);
            Cc[j-1].block(0, 0, 3, 3) = Eigen::MatrixXd::Identity(3, 3);
            Ex.block(num_state_*6+6*num_state_+3*(j-1)*num_state_+3*(i-1),(i)*6,3,6) = Cc[j-1];

                // cam ---- Eu ur
            Eu.block(num_state_*6+num_state_*6+3*(j-1)*num_state_+3*(i-1), num_state_*6+num_state_*3*k_+3*(j-1)*num_state_+3*(i-1), 3, 3) = Eigen::MatrixXd::Identity(3, 3);
            ur.block(3*k_*num_state_+6*num_state_+3*(j-1)*num_state_+3*(i-1),0,3,1) = cam_sol.block((j-1)*3,i,3,1);

            W.block(6*num_state_+num_state_*6+3*(j-1)*num_state_+3*(i-1),6*num_state_+num_state_*6+3*(j-1)*num_state_+3*(i-1),3,3) = paramCAM_.block((j-1)*3,i,3,1).asDiagonal();
        }

        // // noop_est ---- Ex
        // Eigen::MatrixXd Cn;
        // Cn.setZero(3,6);
        // Cn.block(0, 0, 3, 3) = Eigen::MatrixXd::Identity(3,3);
        // Ex.block(num_state_*6+6*num_state_+uwbnum_solveonce*num_state_+3*camnum_solveonce*num_state_+3*(i-1),(i)*6,3,6) = Cn;
        // // noop_est ---- Eu ur W
        // Eu.block(num_state_*6+6*num_state_+uwbnum_solveonce*num_state_+3*camnum_solveonce*num_state_+3*(i-1),num_state_*6+num_state_*3*k_+uwbnum_solveonce*num_state_+3*camnum_solveonce*num_state_+3*(i-1),3,3) = Eigen::MatrixXd::Identity(3,3);
        // ur.block(3*k_*num_state_+6*num_state_+uwbnum_solveonce*num_state_+3*camnum_solveonce*num_state_+3*(i-1), 0,3,1) = uwb_est_sol.col(i*k_);
        // W.block(num_state_*6+6*num_state_+uwbnum_solveonce*num_state_+3*camnum_solveonce*num_state_+3*(i-1),num_state_*6+6*num_state_+uwbnum_solveonce*num_state_+3*camnum_solveonce*num_state_+3*(i-1),3,3) = paramUWB_est_.col(i*k_).asDiagonal();

        // single achor k
        // Eu.block((num_state_)*12+1*(i-1),num_state_*3*k_+6*num_state_+1*(i-1),1,1) = Eigen::MatrixXd::Identity(1,1);  
        // ur(3*k_*num_state_+6*num_state_+1*(i-1),0) = uwb_sol(0,i*k_);
        // ur.block(3*k_*num_state_+6*num_state_+i*4-3,0,3,1) = flow_sol.col(i*k_);
        // W: 320x320 covariance matrix, diag param is set as P,Q,R
        // W.block(6*num_state_+num_state_*6+i*4-4,6*num_state_+num_state_*6+i*4-4,4,4) = R;

        Eigen::MatrixXd tt(1,order_+1);
        for(int j=1;j<=(order_+1);j++){
            tt(0,j-1) = pow(dt_*i*k_,j-1);
        }
        for(int j=1;j<=6;j++){
            t_matrix.block((i)*6+j-1,(j-1)*(order_+1),1,order_+1) = tt;
        }
    } // we can design a member function to perform a shift operation

    // double test_time_start_2 = ros::Time::now().toSec();   //test
    // ROS_INFO("consumptiom dt1: %.4lf seconds", test_time_start_2 - test_time_start_1);

    xtt=x1_.rightCols(x1_.cols()-1);

    for(int j=1;j<=xtt.cols();j=j+1){
        ur.block(0+xtt.rows()*(j-1),0,xtt.rows(),1) = xtt.col(j-1);
    }

    for(int j=1;j<=imu_sol.cols();j=j+1){
        ur.block(xtt.cols()*xtt.rows()+imu_sol.rows()*(j-1),0,imu_sol.rows(),1) = imu_sol.col(j-1);
    }

    Ex_new = Ex * t_matrix;     //320x30
    ar = ((Ex_new.transpose()*W*Ex_new).inverse())*(Ex_new.transpose())*W*Eu*ur;

    // double test_time_start_3 = ros::Time::now().toSec();   //test
    // ROS_INFO("consumptiom dt2: %.4lf seconds", test_time_start_3 - test_time_start_2);

    // Eigen::JacobiSVD<Eigen::MatrixXd> svd2((Ex_new.adjoint()*W*Ex_new).inverse());
    // double cond2 = svd2.singularValues()(0) / svd2.singularValues()(svd2.singularValues().size()-1); 
    // ROS_INFO("cond2: %.4f", cond2);

    xxt_new = t_matrix * ar;

    for(int i=1;i<=num_state_;i++){
        xxe.block(0,i-1,6,1) = xxt_new.block(6*i,0,6,1);
    }

    xt_.col(num_state_)=xxe.rightCols(1);

    xt_real_(0,num_state_) = xt_(0,num_state_) + bias_pos_(0);
    xt_real_(1,num_state_) = xt_(1,num_state_) + bias_pos_(1);
    xt_real_(2,num_state_) = xt_(2,num_state_) + bias_pos_(2);

    xt_real_(3,num_state_) = xt_(3,num_state_);
    xt_real_(4,num_state_) = xt_(4,num_state_);
    xt_real_(5,num_state_) = xt_(5,num_state_);

    // meanFilter
    // int mean_num = 3;
    // Eigen::MatrixXd xt_real_meanFilter = meanFilter(xt_real_.topRows(3).rightCols(mean_num));
    // xt_real_.topRows(3).rightCols(1)= xt_real_meanFilter.rightCols(1);
    // Eigen::MatrixXd xt_meanFilter = meanFilter(xt_.topRows(3).rightCols(mean_num));
    // xt_.topRows(3).rightCols(1)= xt_meanFilter.rightCols(1);

    // use real_est to compare [vicon坐标系]
    // Eigen::MatrixXd xt_real_meanFilter = meanFilterWithSlidingWindow(xt_real_.rightCols(num_state_),3);
    // xt_real_f_.rightCols(1) = xt_real_meanFilter.rightCols(1);

    Eigen::MatrixXd xt_meanFilter = meanFilterWithSlidingWindow(xt_.topRows(3).rightCols(num_state_),3);
    xt_f_.topRows(3).rightCols(1) = xt_meanFilter.rightCols(1);

    // // use relative_est to compare [UWB0坐标系]
    // Eigen::MatrixXd xt_real_meanFilter = meanFilterWithSlidingWindow(xt_.topRows(3).rightCols(num_state_),3);
    // xt_real_f_.topRows(3).rightCols(1) = xt_real_meanFilter.rightCols(1);

    int mean_num = 3;
    for (int i = 1; i <= 3; i++) {
        xt_vel_(i-1,num_state_)=(xt_(i-1,num_state_)-xt_(i-1,num_state_-1))/dt_;
        if(abs(xt_vel_(i-1,num_state_) - xt_vel_(i-1,num_state_-1))>=8.0){
            xt_vel_(i-1,num_state_) = xt_vel_(i-1,num_state_-1);
        }

    }
    Eigen::MatrixXd xt_posf_meanFilter = meanFilter(xt_vel_.rightCols(3));
    xt_f_.bottomRows(3).rightCols(1) = xt_posf_meanFilter.rightCols(1);

    return;

}

//========================callback functions=======================================//
/**
 * @brief callback from csj01 -- dynamic uav
 */
void dwe::pos_uav_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){  // dynamic uav

    VICON_posUAV_msg = *msg;
}

void dwe::vel_uav_cb(const geometry_msgs::TwistStamped::ConstPtr &msg){       // dynamic uav

    VICON_velUAV_msg = *msg;
}


/**
 * @brief callback from cam01-04
 */
void dwe::camA_cb(const geometry_msgs::TransformStamped::ConstPtr &msg){

    cam_cbs_[0] = *msg;
    cam_data_received_[0] = true;
}


void dwe::camB_cb(const geometry_msgs::TransformStamped::ConstPtr &msg){

    cam_cbs_[1] = *msg;
    cam_data_received_[1] = true;
}

void dwe::camC_cb(const geometry_msgs::TransformStamped::ConstPtr &msg){

    cam_cbs_[2] = *msg;
    cam_data_received_[2] = true;
}

void dwe::camD_cb(const geometry_msgs::TransformStamped::ConstPtr &msg){

    cam_cbs_[3] = *msg;
    cam_data_received_[3] = true;
}

void dwe::camA_a2g_cb(const geometry_msgs::TransformStamped::ConstPtr &msg){

    cam_cbs_[4] = *msg;
    cam_data_received_[4] = true;
}

void dwe::camTAG_cb(const geometry_msgs::TransformStamped::ConstPtr &msg){

    cam_cbs_[5] = *msg;
    cam_data_received_[5] = true;
}
